/**
 * @file NewProtocolCurtisNuoli.cpp
 * @author SoBigDog (luke@seer-robotics.com)
 * @brief 
 * @version 0.1
 * @date 2019-11-13
 * 
 * @copyright Copyright (c) 2019
 * 
 */
#include "NewProtocolCurtisNuoli.h"
#include "Console.h"

int16_t CNewProtocolCurtisNuoli::_speedVal 	= 0;
bool CNewProtocolCurtisNuoli::_lift_flag 	= 0;
bool CNewProtocolCurtisNuoli::_lower_flag 	= 0;
int16_t CNewProtocolCurtisNuoli::_forkVel 	= 0;

/**
 * @brief Construct a new CNewProtocolCurtisNuoli::CNewProtocolCurtisNuoli object
 * 
 */
CNewProtocolCurtisNuoli::CNewProtocolCurtisNuoli()
{
	_encoderLine = 1024;
}

/**
 * @brief 参见ProtocolDriver.h
 * 
 * @param ct 
 * @param val 
 * @return true 
 * @return false 
 */
bool CNewProtocolCurtisNuoli::encode(motorDriverCmdType ct, int32_t val)
{
	static bool heartbeat = false;
	if (DRV_CMD_TARGET_SPEED == ct) {
		_speedVal = (int16_t)(val * 60.0f / (4.0f * _encoderLine) / 4000.0f * 4095.0f);
	} else if (DRV_CMD_TARGET_POS == ct) {
		uint8_t fork_ctrl_byte = 0;

		fork_ctrl_byte = ((_lift_flag ? 1 : 0) << 6) + ((_lower_flag ? 1 : 0) << 7);

		CanTxMsg msg;
		msg.StdId = 0x1E5;
		msg.IDE = CAN_Id_Standard;
		msg.RTR = CAN_RTR_Data;
		msg.DLC = 8;

		msg.Data[0] = 0x00;					//
		msg.Data[1] = 0x01;					//AGV调速
		msg.Data[2] = _speedVal & 0xFF;
		msg.Data[3] = _speedVal >> 8;
		msg.Data[4] = 0x00;
		msg.Data[5] = 0x00;
		msg.Data[6] = (_forkVel * 4) & 0xFF;
		msg.Data[7] = (_forkVel * 4) >> 8;
		_canRouter->putMsg(msg);

		msg.StdId = 0x2E5;
		msg.IDE = CAN_Id_Standard;
		msg.RTR = CAN_RTR_Data;
		msg.DLC = 8;

		if (val >= 900) {
			val = 899;
		}
		
		if (val <= -900) {
			val = -899;
		}

		msg.Data[0] = (-val) & 0xFF;
		msg.Data[1] = (-val) >> 8;
		msg.Data[2] = 0x00;
		msg.Data[3] = 0x00;
		msg.Data[4] = 0x00;
		msg.Data[5] = 0x00;
		msg.Data[6] = 0x00;
		msg.Data[7] = 0x00;
		_canRouter->putMsg(msg);
	} else if (DRV_CMD_FORK_VEL == ct) {
		_forkVel = val;
		if (_forkVel > 0) {
			_lift_flag = true;
			_lower_flag = false;
		} else if (_forkVel < 0) {
			_lift_flag = false;
			_lower_flag = true;
		} else if (_forkVel == 0) {
			_lift_flag = false;
			_lower_flag = false;
		}
	} else if (DRV_CMD_ACTURAL_POS == ct) {
		CanTxMsg msg;

		msg.StdId = 0x1E0;
		msg.IDE = CAN_Id_Standard;
		msg.RTR = CAN_RTR_Data;
		msg.DLC = 8;

		msg.Data[0] = 0x00;
		msg.Data[1] = 0x80;
		msg.Data[2] = 0x00;	
		msg.Data[3] = 0x00;	
		msg.Data[4] = 0x00;
		msg.Data[5] = 0x00;		
		msg.Data[6] = 0x00;
		msg.Data[7] = 0x00;	
		_canRouter->putMsg(msg);
	}
	return true;
}

/**
 * @brief 参见ProtocolDriver.h
 * 
 * @param ct 
 * @param val 
 * @param canMsg 
 * @return int 
 */
int CNewProtocolCurtisNuoli::decode(motorDriverCmdType (&ct)[10], int32_t (&val)[10], const CanRxMsg& canMsg)
{
	volatile int16_t rawSpeed 	= 0;
	volatile int16_t rawAngle 	= 0;
	uint8_t walk_error_code 	= 0;
	uint8_t steer_error_code 	= 0;
	uint8_t driver_state		= 0;

	walk_error_code 	= canMsg.Data[0];
	steer_error_code 	= canMsg.Data[1];
	rawAngle 			= (canMsg.Data[3] << 8) + canMsg.Data[2];
	rawSpeed			= (canMsg.Data[5] << 8) + canMsg.Data[4];
	driver_state		= canMsg.Data[7];
	
	if(walk_error_code == 0x59){
			walk_error_code = 0x00;
		}
	if (_motorType == 1) {
		ct[0] = DRV_CMD_ACTURAL_SPEED;
		ct[1] = DRV_CMD_AUTO_MODE;
		ct[2] = DRV_CMD_DRI_ERR;
		ct[3] = DRV_CMD_DRI_ERRCODE;

		val[0] = -rawSpeed;
		val[1] = (driver_state & 0x01) ? 0 : 1;
		val[2] = (walk_error_code > 0) ? 1 : 0;
		val[3] = walk_error_code;

		return 4;
	} else if (_motorType == 2) {
		ct[0] = DRV_CMD_ACTURAL_POS;
		ct[1] = DRV_CMD_AUTO_MODE;
		ct[2] = DRV_CMD_DRI_ERR;
		ct[3] = DRV_CMD_DRI_ERRCODE;

		val[0] = (-rawAngle) / 16384.0f * 900.0f;
		val[1] = (driver_state & 0x01) ? 0 : 1;
		val[2] = (steer_error_code > 0) ? 1 : 0;
		val[3] = steer_error_code;

		return 4;
	} else if (_motorType == 6) {
		ct[0] = DRV_CMD_AUTO_MODE;
		val[0] = (driver_state & 0x01) ? 0 : 1;

		return 1;
	} else {
		return 0;
	}

}

